1. Field of the Invention
The present invention relates to a control method of a robot and a machine tool and, more specifically, to a method which prevents vibration from occurring at the tip of a robot arm, a machining point of a workpiece attached to a machine tool, or a tool position when the robot or machine tool is controlled by a semi=closed loop.
2. Description of the Related Art
In robots and machine tools, there are provided detectors for detecting the position and speed of servomotors for driving a robot or a machine tool. The position and speed of a final control object, i.e., the tip of a robot arm (tool center point), a work, a tool, or the like are controlled by controlling the position and speed of the servomotors. That is, the control of robots and machine tools is usually performed by a semi-closed loop.
However, the servomotor is linked to the control object such as the tip of a robot arm, a workpiece or a tool through speed reducers, arms and other members. That is, the mechanism from the motor to the control object is not regarded as a complete rigid body. As a result, there some times occurs an overshoot or vibration at the control object such as an arm tip during acceleration or deceleration.
FIG. 3 is a schematic diagram of a spring and damper system which is modelled to represent a vibration system including an arm, a speed reducer, etc. In FIG. 3, .theta.L represents the position of a control object such as an arm tip; .theta.m, the position of a motor; JL, load inertia; Jm, motor inertia; K, a spring constant; D, a viscosity coefficient; and u, torque input (motor output torque) that is inputted to this vibration system. An equation of motion with respect to the motor is expressed as follows: EQU u=Jm.multidot..theta.m+D(.theta.m-.theta.L)+K(.theta.m-.theta.L)(1)
In the above equation (1), a symbol with two dots indicates a second-order derivative, and a symbol with one dot indicates a first-order derivative. For example, .theta.m indicates a second-order derivative of the motor position .theta.m and represent the acceleration of the motor. .theta.m indicates a first-order derivative of the motor position .theta.m and represents the speed of the motor.
On the other hand, an equation of motion with respect to the load is expressed as follows: EQU 0=JL.multidot..theta.L+D(.theta.L-.theta.m)+K(.theta.L-.theta.m)(2)
The following equation (3) is obtained from the above equation (2): ##EQU1## where s is the Laplace operator.
The response characteristic according to the equation (3) from the motor position .theta.m to the position .theta.L of the control object such as an arm tip indicates that a displacement such as a bend or twist occurs between the motor position .theta.m and the control object position .theta.L during acceleration or deceleration. Therefore, it is impossible to avoid vibration at the control object position .theta.L even if the motor position .theta.m is controlled correctly.
Therefore, it is necessary to set the position loop gain smaller than the gain corresponding to the natural frequency determined by the equation (3). Alternatively, the mechanisms need to be so designed as to increase the natural frequency.
From the equations (1) and (2), a transfer function from the torque input u to the acceleration of the control object such as an arm tip is expressed by the following equation (4). ##EQU2##
This transfer function shows that the system is a second-order vibration system. Therefore, in forming a control loop for the position or speed of the control object such as an arm tip, if the band (natural frequency) of equation (4) is low, it is impossible to obtain a sufficiently high band of the speed loop and, therefore, it is impossible either to obtain a sufficiently high band of the position loop.
However, in the prior art, a control loop is formed so that the response characteristic from the torque input u to the acceleration of the control point is indicated by the transfer function as follows: ##EQU3##
Alternatively, a control loop is formed with an assumption that there is no displacement such as a bend or twist between the motor position and the control object, i.e., .theta.L=.theta.m. Therefore, as described above, it is difficult to prevent vibration from occurring at the control object such as an arm tip.